﻿Imports System.Text
Imports System.ComponentModel
Imports System.Threading
Imports System.Runtime.InteropServices

Public Class WingF1 : Inherits Transport
#Region "Frame ID"
    Public Const ID_GET_ANGLE As Byte = &H1
    Public Const ID_SET_ZERO As Byte = &H2
#End Region

    'Public FrameGetAngles As Byte() = {&HD, &HA, &H1, &H1, &H1, &H1, &H1, &H0, &H0, &H5, &H5A, &HA5, &HD, &HA, &H1, &H1, &H1, &H1, &H1, &H0, &H0, &H25, &H5A, &HA5}
    Public FrameGetAngles As Byte() = {&HD, &HA, &H1, &H1, &H1, &H1, &H1, &H0, &H0, &H5, &H5A, &HA5}
    Public Angles As GetAngles_T
    Delegate Sub ParseHandler(frame As Byte())
    Public MsgHandlers(10) As ParseHandler

    ' 每个消息类型数据区的长度
    Private ReadOnly AckLength As Dictionary(Of Byte, Integer) = New Dictionary(Of Byte, Integer) From {
        {ID_GET_ANGLE, 19},
        {ID_SET_ZERO, 7}
    }

    Public Sub New()
        ReDim Angles.Angle(3)
        MsgHandlers(ID_GET_ANGLE) = AddressOf ParseAngles
    End Sub

    Public Sub GetAngles()
        If Me.Ready Then
            SendMsg(FrameGetAngles)
            Parse(ReadAck())
        End If
    End Sub

    Public Sub SetZero(num As UShort)
        If num > 4 Then Return
        Dim base As Byte() = {&HD, &HA, &H2, &H0, &H0, &H0, &H0, &H0, &H0, &H3, &H5A, &HA5}
        base(2 + num) = 1
        If Me.Ready Then
            SendMsg(base)
            Parse(ReadAck())
        End If
    End Sub

    Public Sub ParseAngles(frame As Byte())
        Angles = Transport.ByteToStruct(frame, Angles.GetType())
    End Sub

    Dim print_byte As Action(Of Byte()) = Sub(bs As Byte())
                                              For Each b As Byte In bs
                                                  Console.Write(b.ToString("X2") + " ")
                                              Next
                                              Console.Write(vbCrLf)
                                          End Sub

    Public Function Parse(frame As Byte()) As Boolean

        Dim ret As Boolean = True
        Dim frame_data As Byte()

        Dim crc_sum = Function(bs As Byte()) As Byte
                          Dim sum As UInteger = 0
                          For Each b In bs
                              sum += b
                          Next
                          Return sum Mod 256
                      End Function

        If frame IsNot Nothing Then
            If frame.Length >= 6 Then
                print_byte(frame)
                If frame(frame.Length - 1) = &HA5 Then
                    ReDim frame_data(frame.Length - 6)
                    Array.Copy(frame, 2, frame_data, 0, frame_data.Length)

                    Dim crc_check As Byte = crc_sum(frame_data)

                    Dim crc_get As Byte = frame(frame.Length - 3)

                    If (crc_get <> crc_check) Then
                        Console.WriteLine("CRC Error.")
                        ret = False
                    Else
                        If (MsgHandlers(frame(2)) IsNot Nothing) Then
                            MsgHandlers(frame(2)).Invoke(frame_data)
                        End If
                    End If
                Else
                    Console.WriteLine("Error Tail.")
                    ret = False
                End If
            Else
                Console.WriteLine("Error Length, Frame.Length < 6.")
            End If
        Else
            Console.WriteLine("Empty Message.")
            ret = False
        End If

        Return ret

    End Function

    Public Function ReadAck() As Byte()
        Dim data() As Byte = Nothing

        Dim drop_data = Sub()
                            Dim byte_left(port.BytesToRead - 1) As Byte
                            port.Read(byte_left, 0, byte_left.Length)
                            print_byte(byte_left)
                            port.DiscardInBuffer()
                        End Sub

        Console.WriteLine("------------ wait ack ------------")
        Dim start_time As Long = Now.Ticks()
        Dim len As Integer = 0
        Try
            '1ticks = 100纳秒 = 0.1微秒
            While Now.Ticks() - start_time < 10000 * 200
                If len = 0 Then
                    If port.BytesToRead >= 3 Then
                        Dim head(2) As Byte
                        port.Read(head, 0, head.Length)
                        If head(0) = &HD Then
                            ' 计算余下的长度
                            len = AckLength(head(2)) + 2
                            ReDim data(len + 2)
                            head.CopyTo(data, 0)
                        Else
                            Console.WriteLine("Error Head.")
                            drop_data()
                            Return Nothing
                        End If
                    End If
                Else
                    If port.BytesToRead >= len Then
                        port.Read(data, 3, len)
                        port.DiscardInBuffer()
                        Return data
                    End If
                End If

            End While
            Console.WriteLine($"[Warn] Read Timeout {(Now.Ticks() - start_time) / 10000.0}ms")
            drop_data()

        Catch ex As Exception
            Console.WriteLine($"[Error] {ex.Message}")
        End Try

        Return Nothing
    End Function

    ''' <summary>
    ''' 下发协议
    ''' </summary>
    ''' <param name="buf">协议内容</param>
    ''' <returns></returns>
    Public Function SendMsg(buf() As Byte) As Boolean
        Console.WriteLine("------------ I send ------------")

        '发送缓存数据
        For Each b As Byte In buf
            Console.Write(b.ToString("X2") + " ")
        Next
        Console.Write(vbCrLf)

        Dim ret As Boolean = True
        If buf.Length < 5 Then
            ret = False
        Else
            If buf(0) <> &HD Then
                ret = False
            Else
                port.Write(buf, 0, buf.Length)
            End If
        End If
        Return ret
    End Function

End Class

<StructLayout(LayoutKind.Sequential, CharSet:=CharSet.Auto, Pack:=1)>
Public Structure GetAngles_T

    Public Id As Byte

    <MarshalAs(UnmanagedType.ByValArray, SizeConst:=4)>
    Public Angle() As Integer

    Public Preserved As UShort

End Structure

